#ifndef __MATRIX_3X3_H__
#define __MATRIX_3X3_H__

#pragma  once

#include "Vector3.h"
#include "NezhaException.h"

namespace Nezha
{

	class NEZHA_ENGINE_ENTRY Matrix3x3
	{
	public:
		Matrix3x3()
		{
			// without initialize
		}

		Matrix3x3(const Matrix3x3 &rhs)
		{
			memcpy(mEntries, rhs.mEntries, sizeof(mEntries));
		}

		Matrix3x3(Real m00, Real m01, Real m02,
			Real m10, Real m11, Real m12,
			Real m20, Real m21, Real m22)
		{
			mEntries[0] = m00;
			mEntries[1] = m01;
			mEntries[2] = m02;
			mEntries[3] = m10;
			mEntries[4] = m11;
			mEntries[5] = m12;
			mEntries[6] = m20;
			mEntries[7] = m21;
			mEntries[8] = m22;
		}

		Matrix3x3(const Vector3 &u, const Vector3 &v, const Vector3 &w, bool bColumnajor = true)
		{
			if(bColumnajor)
			{
				setCol(0, u);
				setCol(1, v);
				setCol(2, w);
			}
			else
			{
				setRow(0, u);
				setRow(1, v);
				setRow(2, w);
			}
		}

		const Real* operator[] (u32 iRow) const
		{
			NZ_ASSERT(iRow < 3);
			return &mEntries[3*iRow];
		}

		Real* operator[] (u32 iRow)
		{
			NZ_ASSERT(iRow < 3);
			return &mEntries[3*iRow];
		}

		// row
		Vector3& getRow(u32 iRow, Vector3 &row) const
		{
			NZ_ASSERT(iRow < 3);
			int i = iRow * 3;
			row.x = mEntries[i];
			row.y = mEntries[++i];
			row.z = mEntries[++i];

			return row;
		}

		void setRow(u32 iRow, const Vector3 &row)
		{
			NZ_ASSERT(iRow < 3);
			int i = iRow * 3;
			mEntries[i] = row.x;
			mEntries[++i] = row.y;
			mEntries[++i] = row.z;
		}

		Real* getRow(u32 iRow, Real *pArray) const
		{
			NZ_ASSERT(iRow < 3);
			int i = iRow * 3;
			pArray[0] = mEntries[i];
			pArray[1] = mEntries[++i];
			pArray[2] = mEntries[++i];

			return pArray;
		}

		void setRow(u32 iRow, const Real *pArray)
		{
			NZ_ASSERT(iRow < 3);
			int i = iRow * 3;
			mEntries[i] = pArray[0];
			mEntries[++i] = pArray[1];
			mEntries[++i] = pArray[2];
		}

		void getRow(u32 iRow, Real &f0, Real &f1, Real &f2) const
		{
			NZ_ASSERT(iRow < 3);
			int i = iRow * 3;
			f0 = mEntries[i];
			f1 = mEntries[++i];
			f2 = mEntries[++i];
		}

		void setRow(u32 iRow, Real f0, Real f1, Real f2)
		{
			NZ_ASSERT(iRow < 3);
			int i = iRow * 3;
			mEntries[i] = f0;
			mEntries[++i] = f1;
			mEntries[++i] = f2;
		}

		// column
		Vector3& getCol(u32 iCol, Vector3 &col) const
		{
			NZ_ASSERT(iCol < 3);
			col.x  = mEntries[iCol];
			col.y  = mEntries[3 + iCol];
			col.z  = mEntries[6 + iCol];

			return col;
		}

		void setCol(u32 iCol, const Vector3 &col)
		{
			NZ_ASSERT(iCol < 3);
			mEntries[iCol] = col.x;
			mEntries[3 + iCol] = col.y;
			mEntries[6 + iCol] = col.z;
		}

		Real* getCol(u32 iCol, Real *pArray) const
		{
			NZ_ASSERT(iCol < 3);
			pArray[0] = mEntries[iCol];
			pArray[1] = mEntries[3 + iCol];
			pArray[2] = mEntries[6 + iCol];

			return pArray;
		}

		void setCol(u32 iCol, const Real *pArray)
		{
			NZ_ASSERT(iCol < 3);
			mEntries[iCol] = pArray[0];
			mEntries[3 + iCol] = pArray[1];
			mEntries[6 + iCol] = pArray[2];
		}
		void getCol(u32 iCol, Real &f0, Real &f1, Real &f2) const
		{
			NZ_ASSERT(iCol < 3);
			f0 = mEntries[iCol];
			f1 = mEntries[3 + iCol];
			f2 = mEntries[6 + iCol];
		}

		void setCol(u32 iCol, Real f0, Real f1, Real f2)
		{
			NZ_ASSERT(iCol < 3);
			mEntries[iCol] = f0;
			mEntries[3 + iCol] = f1;
			mEntries[6 + iCol] = f2;
		}

		Real get(u32 iRow, u32 iCol) const
		{
			NZ_ASSERT(iRow < 3 && iCol < 3);
			return mEntries[3 * iRow + iCol];
		}

		void set(u32 iRow, u32 iCol, Real fVal)
		{
			NZ_ASSERT(iRow < 3 && iCol < 3);
			mEntries[3 * iRow + iCol] = fVal;
		}

		void makeZero()
		{
			setRow(0, 0, 0, 0);
			setRow(1, 0, 0, 0);
			setRow(2, 0, 0, 0);
		}

		void makeIdentity()
		{
			mEntries[0] = 1.0;
			mEntries[1] = 0.0;
			mEntries[2] = 0.0;
			mEntries[3] = 0.0;
			mEntries[4] = 1.0;
			mEntries[5] = 0.0;
			mEntries[6] = 0.0;
			mEntries[7] = 0.0;
			mEntries[8] = 1.0;
		}

		void makeDiagonal(Real fDiag0, Real fDiag1, Real fDiag2)
		{
			mEntries[0] = fDiag0; mEntries[1] = mEntries[2] = 0.0;
			mEntries[3] = 0.0; mEntries[4] = fDiag1; mEntries[5] = 0.0;
			mEntries[6] = mEntries[7] = 0.0; mEntries[8] = fDiag2;
		}

		void xRotation(Real fAngle);
		void yRotation(Real fAngle);
		void zRotation(Real fAngle);
		Matrix3x3& fromAxisAngle(Real fAngle, Real x, Real y, Real z);
		Matrix3x3& fromAxisAngle(Real fAngle, const Vector3 &axis)
		{
			return fromAxisAngle(fAngle, axis.x, axis.y, axis.z);
		}

		Matrix3x3& makeTensorProduct(const Vector3 &u, const Vector3 &v);

		bool operator== (const Matrix3x3 &mat) const;
		bool operator!= (const Matrix3x3 &mat) const
		{
			return !(*this == mat);
		}

		Matrix3x3 operator+ (const Matrix3x3 &mat) const;
		Matrix3x3 operator- (const Matrix3x3 &mat) const;
		Matrix3x3 operator* (const Matrix3x3 &mat) const;
		Matrix3x3 operator* (Real fScalar) const;

		Vector3 operator* (const Vector3 &pt) const
		{
			return Vector3(
				mEntries[0]*pt.x + mEntries[1]*pt.y + mEntries[2]*pt.z,
				mEntries[3]*pt.x + mEntries[4]*pt.y + mEntries[5]*pt.z,
				mEntries[6]*pt.x + mEntries[7]*pt.y + mEntries[8]*pt.z);
		}

		// use for v^T * mat
		friend Vector3 operator*(const Vector3 &pt, const Matrix3x3 &mat);

		Matrix3x3 inverse() const;
		bool inverse(Matrix3x3 &inv) const;
		Matrix3x3 transpose() const;

		void toAngleAxis(Real &fAngle, Real &x, Real &y, Real &z) const;

		/** Gram-Schmidt orthogonalization */
		bool orthogonalize();

		/** this^T * mat */
		Matrix3x3 transposeTimes(const Matrix3x3 &mat) const;

		/** this*D */
		Matrix3x3 timesDiagonal(const Vector3 &diag) const;

		Real determinant() const;

		bool toEulerAnglesXYZ(Real &fX, Real &fY, Real &fZ) const;
		bool toEulerAnglesXZY(Real &fX, Real &fZ, Real &fY) const;
		bool toEulerAnglesYXZ(Real &fY, Real &fX, Real &fZ) const;
		bool toEulerAnglesYZX(Real &fY, Real &fZ, Real &fX) const;
		bool toEulerAnglesZXY(Real &fZ, Real &fX, Real &fY) const;
		bool toEulerAnglesZYX(Real &fZ, Real &fY, Real &fX) const;
		void fromEulerAnglesXYZ(Real fX, Real fY, Real fZ);
		void fromEulerAnglesXZY(Real fX, Real fZ, Real fY);
		void fromEulerAnglesYXZ(Real fY, Real fX, Real fZ);
		void fromEulerAnglesYZX(Real fY, Real fZ, Real fX);
		void fromEulerAnglesZXY(Real fZ, Real fX, Real fY);
		void fromEulerAnglesZYX(Real fZ, Real fY, Real fX);

			// TODO eigensolving

	private:
			Real mEntries[9];

	public:
			static const Matrix3x3 ZERO;
			static const Matrix3x3 IDENTITY;
		};

		inline Vector3 operator* (const Vector3 &pt, const Matrix3x3 &mat)
		{
			return Vector3(
				pt.x*mat.mEntries[0] + pt.y*mat.mEntries[3] + pt.z*mat.mEntries[6],
				pt.x*mat.mEntries[1] + pt.y*mat.mEntries[4] + pt.z*mat.mEntries[7],
				pt.x*mat.mEntries[2] + pt.y*mat.mEntries[5] + pt.z*mat.mEntries[8]);
		}

}//end namespace Nezha

#endif //end __MATRIX_3X3_H__